# @Time : 2025-01-17 11:14
# @Author : Fioman 
# @Phone : 13149920693
"""
图像处理帮助类,封装了一些共用的API
"""
import math
from typing import Tuple

import cv2 as cv
import numpy as np

from params.camera_params import CameraParams
from params.robot_params import RobotParams
from vision_process import calc_config

cp: CameraParams = CameraParams()
rp: RobotParams = RobotParams()


def image_show(name, image):
    if calc_config.showState:
        sizeScale = 6
        h, w = image.shape[:2]
        newWidth, newHeight = int(w / sizeScale), int(h / sizeScale)
        showImage = cv.resize(image, (newWidth, newHeight), cv.INTER_AREA)
        cv.namedWindow(str(name))
        cv.imshow(str(name), showImage)
        cv.waitKey(0)


def get_size_scale_image(image, sizeScale=2):
    """
    按照比例缩放后的图像,获取.比例>1,代表是缩小,小于1则代表是放大,函数主要用来缩小图像
    :param image:
    :param sizeScale: 缩小比例
    :return:
    """
    h, w = image.shape[:2]
    newWidth, newHeight = int(w / sizeScale + 0.5), int(h / sizeScale + 0.5)
    newImage = cv.resize(image, (newWidth, newHeight), interpolation=cv.INTER_AREA)
    return newImage


def get_rect_points_clockwise(rectPoints):
    """
    根据矩形的四个顶点,获取其顺时针方向的四个顶点
    :param rectPoints:
    :return:
    """
    rectPointsSorted = sorted(rectPoints, key=np.sum)
    leftTop = rectPointsSorted[0]
    rightBottom = rectPointsSorted[-1]

    if rectPointsSorted[1][0] > rectPointsSorted[2][0]:
        rightTop = rectPointsSorted[1]
        leftBottom = rectPointsSorted[2]
    else:
        rightTop = rectPointsSorted[2]
        leftBottom = rectPointsSorted[1]

    return tuple(leftTop), tuple(rightTop), tuple(rightBottom), tuple(leftBottom)


def get_two_point_distance(p1, p2):
    """
    获取两点之间的距离,保留小数点后两位
    :param p1: 点1
    :param p2: 点2
    :return:
    """
    assert len(p1) == 2
    assert len(p2) == 2
    ds = float(np.sqrt((p2[0] - p1[0]) ** 2 + (p2[1] - p1[1]) ** 2))
    return round(ds, 2)


def get_contour_is_rect_ratio(cnt):
    """
    获取轮廓的近似度
    :param cnt:
    :return:
    """
    boxMaster = cv.minAreaRect(cnt)
    _, (w, h), _ = boxMaster
    ratio = cv.contourArea(cnt) / (w * h)
    return round(ratio, 5)


def get_board_without_angle(image, boxMaster, extend=0):
    """
    获取无角度的区域,是否延长,表示是否延长物料区域的四周,可以完整的看到物料区域,extend表示延长多少的意思
    extend = 0 表示不延长
    :param image:
    :param boxMaster:
    :param extend:
    :return:
    """
    (centerX, centerY), (w, h), angle = boxMaster
    widthHalf = int(w / 2)
    heightHalf = int(h / 2)
    yStart = max(int(centerY - heightHalf), 0)  # 之所以有零的情况,是因为旋转之后物料框的y起点可能跑到外面去了.
    yEnd = min(int(centerY + heightHalf), image.shape[0])  # yEnd也是一样的,有可能出去了
    xStart = max(int(centerX - widthHalf), 0)
    xEnd = min(int(centerX + widthHalf), image.shape[1])
    M = cv.getRotationMatrix2D((centerX, centerY), angle, 1)
    if extend != 0:
        if yStart - extend > 0:
            yStart -= extend
        if yEnd + extend < image.shape[0]:
            yEnd += extend
        if xStart - extend > 0:
            xStart -= extend
        if xStart + extend < image.shape[1]:
            xEnd += extend
    h, w = image.shape[:2]
    totalboard = cv.warpAffine(image, M, (w, h))[yStart:yEnd, xStart:xEnd]
    return totalboard, [xStart, yStart]


def get_line_k_and_b(p1, p2):
    """
    根据两个点获取这条线的斜率和斜距
    :param p1: 线段的点1
    :param p2: 线段的点2
    :return:
    """
    if p1[0] == p2[0]:
        # 垂直于x轴,没有k值,斜率是None
        return None, p1[0]
    else:
        k = (p2[1] - p1[1]) / (p2[0] - p1[0])
        b = p1[1] - k * p1[0]
        return k, b


def get_two_lines_intersection(k1, b1, k2, b2) -> Tuple[int | None, int | None]:
    """
    获取两条直线的交点
    :param k1: 第一条直线的斜率
    :param b1: 第一条直线的斜距
    :param k2: 第二条直线的斜率
    :param b2: 第二条直线的斜距
    :return: 返回两条直线的交点
    """
    if k1 == k2:
        return None, None
    elif k1 is None and k2 is not None:
        x = b1
        y = k2 * x + b2
        return int(x + 0.5), int(y + 0.5)
    elif k2 is None and k1 is not None:
        x = b2
        y = k1 * x + b1
        return int(x + 0.5), int(y + 0.5)
    else:
        x = (b1 - b2) / (k1 - k2) * (-1)
        y = k1 * x + b1
        return int(x + 0.5), int(y + 0.5)


def get_point_to_line_distance(p, lineK, lineB, directionBy="y"):
    """
    获取点到直线的距离,默认是按照y方向
    :param p: 要获取到直线的距离的点
    :param lineK: 直线的斜率K
    :param lineB: 直线的斜距B
    :param directionBy 正负方向问题
    :return:
    """
    if lineK == 0:
        # 如果直线是水平的,那么它的垂线是垂直x轴的,也就是没有斜率.
        verLineK = None
        verLineB = p[0]
    elif lineK is None:
        verLineK = 0
        verLineB = p[1]
    else:
        # 垂线和原来的直线的斜率的乘积是-1
        verLineK = -1 / lineK
        verLineB = p[1] - verLineK * p[0]

    # 然后根据垂线的斜率和斜距以及原来的线的斜率和斜距来获取交点
    intersection = get_two_lines_intersection(lineK, lineB, verLineK, verLineB)
    # 然后是获取两个点的距离,距离有正负,如果检测的直线,在传感器点的下方为正,在上方为负
    dis = get_two_point_distance(p, intersection)
    if directionBy == "y":
        dis = dis if p[1] > intersection[1] else -dis
    else:
        dis = dis if p[0] > intersection[0] else -dis
    return round(dis, 2)


def get_two_points_middle(p1, p2):
    """
    获取两个点的中点坐标
    :param p1: 点1的坐标
    :param p2: 点2的坐标
    :return:
    """
    assert len(p1) == 2
    assert len(p2) == 2
    middleX = (p1[0] + p2[0]) / 2
    middleY = (p1[1] + p2[1]) / 2
    return int(middleX + 0.5), int(middleY + 0.5)


def get_width_height_by_image_name(imageName):
    """
    根据图像名称获取宽和高
    :param imageName:
    :return:
    """

    try:
        if "w_" not in imageName or "h_" not in imageName:
            return 1500, 1500
        imageName = imageName[:-4]

        widthIndex = imageName.index("w_")
        heightIndex = imageName.index("h_")
        newW = imageName[widthIndex + 2:widthIndex + 6]
        newH = imageName[heightIndex + 2:heightIndex + 6]
    except Exception as e:
        print(str(e))
        return None, None
    return int(newW), int(newH)


def get_angle_by_k(k):
    """
    根据k值去获取角度,单位是°
    :param k:
    :return:
    """
    if k == 0:
        return 0
    if k is None:
        return 90
    return np.arctan(k) * 180 / np.pi


def get_line_k(p1, p2):
    """
    获取某一条直线的斜率
    :param p1:点1的坐标
    :param p2:点2的坐标
    :return:
    """
    if p1[0] == p2[0]:
        # 垂直于x轴，没有K值
        return None
    else:
        return (p2[1] - p1[1]) * 1.0 / (p2[0] - p1[0])


# def check_point_in_rect(point, box):
#     """
#     判断的规则是:
#     利用的夹角的正负值,可以确定是顺时针还是逆时针方向.如果在矩形的内部的话,肯定是全部大于90度或者全部小于90度.
#     这样就可以利用向量的点积的正负号来判断夹角是大于90度还是小于90度.
#     判断一个点是否在矩形内,box是识别出来的一个框的矩形,point是另外一个框的一个点.
#     :param point:矩形框的四个顶点的某一个点
#     :param box:另外一个矩形框的四个顶点
#     :return:
#     """
#     x, y = point
#     Ax, Ay = box[0]
#     Bx, By = box[3]
#     Cx, Cy = box[2]
#     Dx, Dy = box[1]
#     a = (Bx - Ax) * (y - Ay) - (By - Ay) * (x - Ax)
#     b = (Cx - Bx) * (y - By) - (Cy - By) * (x - Bx)
#     c = (Dx - Cx) * (y - Cy) - (Dy - Cy) * (x - Cx)
#     d = (Ax - Dx) * (y - Dy) - (Ay - Dy) * (x - Dx)
#     if ((a > 0 and b > 0 and c > 0 and d > 0) or (a < 0 and b < 0 and c < 0 and d < 0)):
#         return True
#     else:
#         return False


def check_two_box_is_overlap(box1, box2):
    """
    判断两个矩形是否有重合区域
    :param box1: 第一个矩形的四个顶点坐标 [(x1,y1), (x2,y2), (x3,y3), (x4,y4)]
    :param box2: 第二个矩形的四个顶点坐标 [(x1,y1), (x2,y2), (x3,y3), (x4,y4)]
    :return: 是否重叠
    """
    isOverlap = False
    # 首先是判断是否叠板
    for point in box1:
        if check_point_in_rect(point, box2):
            isOverlap = True

    for point in box2:
        if check_point_in_rect(point, box1):
            isOverlap = True
    return isOverlap


def check_two_box_is_overlap_old(upBox, downBox):
    """
    判断两个矩形是否有重合区域,也就是折叠区域
    :param upBox: 上框的四个顶点
    :param downBox: 下框的四个顶点
    :return:
    """
    isOverlap = False
    for point in upBox:
        if check_point_in_rect(point, downBox):
            isOverlap = True

    for point in downBox:
        if check_point_in_rect(point, upBox):
            isOverlap = True
    return isOverlap


def check_point_in_rect(point, box):
    """
        判断的规则是:
        利用的夹角的正负值,可以确定是顺时针还是逆时针方向.如果在矩形的内部的话,
        肯定是全部大于90度或者全部小于90度.
        这样就可以利用向量的点积的正负号来判断夹角是大于90度还是小于90度.
        判断一个点是否在矩形内,box是识别出来的一个框的矩形,point是另外一个框的一个点.
        :param point:矩形框的四个顶点的某一个点
        :param box:另外一个矩形框的四个顶点
        :return:
        """
    x, y = point
    Ax, Ay = box[0]
    Bx, By = box[3]
    Cx, Cy = box[2]
    Dx, Dy = box[1]
    a = (Bx - Ax) * (y - Ay) - (By - Ay) * (x - Ax)
    b = (Cx - Bx) * (y - By) - (Cy - By) * (x - Bx)
    c = (Dx - Cx) * (y - Cy) - (Dy - Cy) * (x - Cx)
    d = (Ax - Dx) * (y - Dy) - (Ay - Dy) * (x - Dx)
    if (a > 0 and b > 0 and c > 0 and d > 0) or (a < 0 and b < 0 and c < 0 and d < 0):
        return True
    else:
        return False


def get_sum_abs(iterVal):
    """
    计算可迭代对象的序列绝对值的和
    :param iterVal: 可迭代对象
    :return:
    """
    return sum([abs(x) for x in iterVal])


def get_horizontal_middle_line_4p(upBottomPoints, downTopPoints):
    """
    根据8个顶点,获取4拼板的中线
    :param upBottomPoints:
    :param downTopPoints:
    :return:
    """
    upK, upB = get_linear_fit_k_and_b_horizontal(np.array(upBottomPoints))
    downK, downB = get_linear_fit_k_and_b_horizontal(np.array(downTopPoints))
    interSection = get_two_lines_intersection(upK, upB, downK, downB)
    if upK == downK or interSection[0] is None:
        middleLineK = upK
        middleLineP = get_two_points_middle(get_two_points_middle(upBottomPoints[0], upBottomPoints[-1]),
                                            get_two_points_middle(downTopPoints[0], downTopPoints[-2]))
        middleLineB = middleLineP[1] - upK * middleLineP[0]
    else:
        middleLineK = np.tan((np.arctan(upK) + np.arctan(downK)) / 2)
        middleLineB = interSection[1] - middleLineK * interSection[0]

    return middleLineK, middleLineB


def get_linear_fit_k_and_b_horizontal(pointsMat):
    """
    垂直方向获取拟合直线,垂直方向上,有角度可能接近90度,如果大于一定的值,就认为是None
    :param pointsMat: 要拟合的直线的四个点
    :return:
    """
    outPut = cv.fitLine(pointsMat, cv.DIST_L2, 0, 0.01, 0.01)
    if outPut[0] != 0:
        k = outPut[1] / outPut[0]
        k = k[0]
        if abs(k) > 1146:
            k = None
        b = outPut[3] - k * outPut[2]
    else:
        k = None
        b = outPut[2]
    return float(k), float(b)


def get_vertical_middle_cut_line_4p(leftLine, rightLine):
    """
    获取中线,垂直中线.做法就是求对应点的中点,然后连线.
    :param leftLine:
    :param rightLine:
    :return:
    """
    p1, p2 = leftLine
    p3, p4 = rightLine
    upMiddle = np.add(p1, p3) / 2
    downMiddle = np.add(p2, p4) / 2
    verMidK, verMidB = get_line_k_and_b(upMiddle, downMiddle)
    return verMidK, verMidB


def get_two_line_distance(innerLine, outterLine):
    """
    获取两条直线的距离,按照第二条线的中点做垂线,然后和第一条线的交点来作为两条直线的距离的评判标准
    :param innerLine: 内层板找到的一条线
    :param outterLine: 物料区域的外层的一条线
    :return:
    """
    innerK, innerB = get_line_k_and_b(innerLine[0], innerLine[1])
    outterK, outterB = get_line_k_and_b(outterLine[0], outterLine[1])
    outterMid = get_two_points_middle(outterLine[0], outterLine[1])

    if outterK == 0:
        # 求外层线中点的垂线和内层线的交点
        intersectionX = outterMid[0]
        intersectionY = innerK * intersectionX + innerB
    elif outterK is None:
        # 如果外层线是垂直的情况,它的垂线的斜率是0,并且过outterMid这个点
        verLineK = 0
        verLineB = outterMid[1]
        intersectionX, intersectionY = get_two_lines_intersection(verLineK, verLineB, innerK, innerB)
    else:
        verLineK = -1 / outterK
        verLineB = outterMid[1] - verLineK * outterMid[0]
        intersectionX, intersectionY = get_two_lines_intersection(verLineK, verLineB, innerK, innerB)

    lineDis = get_two_point_distance(outterMid, (intersectionX, intersectionY))
    return round(lineDis, 2)


def transform_point_to_big(point, M, offset):
    """
    将局部坐标系中的点转换到源图像坐标系
    Args:
        point: 局部坐标系中的点 (x,y)
        M: 旋转矩阵
        offset: 中心点偏移量 (x,y)
    Returns:
        转换后的点坐标 (x,y)
    """
    x, y = point
    transformed = np.dot(M, [x + offset[0], y + offset[1], 1])
    return int(transformed[0]), int(transformed[1])


def get_bisecor_line_kb_vertical(p1, p2, p3, p4):
    upMiddle = np.add(p1, p3) / 2
    downMiddle = np.add(p2, p4) / 2
    verMidK, verMidB = get_line_k_and_b(upMiddle, downMiddle)
    return verMidK, verMidB


def get_bisecor_line_kb(p1, p2, p3, p4):
    """
    根据四个点获取角平分线
    :param p1:
    :param p2:
    :param p3:
    :param p4:
    :return:
    """
    upK, upB = get_line_k_and_b(p1, p2)
    downK, downB = get_line_k_and_b(p3, p4)
    angle1 = get_angle_by_k(upK)
    angle2 = get_angle_by_k(downK)
    if abs(abs(angle1) - abs(angle2)) < 0.01:
        intersection = [None, None]
    else:
        intersection = get_two_lines_intersection(upK, upB, downK, downB)
    # 如果本身是平行线,没有交点的时候
    if intersection[0] is None:
        middleLineK = upK
        middleLineP = np.add(p1, p3) / 2
        middleLineB = middleLineP[1] - middleLineK * middleLineP[0]
    else:
        middleLineK = np.tan((np.arctan(upK) + np.arctan(downK)) / 2)  # 角平分线公式
        middleLineB = intersection[1] - middleLineK * intersection[0]
    return middleLineK, middleLineB


class Line(object):
    def __init__(self, x1, y1, x2, y2):
        self.x1 = x1
        self.y1 = y1
        self.x2 = x2
        self.y2 = y2

    def vector(self):
        c = (self.x1 - self.x2, self.y1 - self.y2)
        return c

    def length(self):
        length = math.sqrt(self.vector()[0] * self.vector()[0] + self.vector()[1] * self.vector()[1])
        return length


def get_two_line_angle(line1, line2):
    """
    获取第一条线到第二条线的角度,站到裁切机的原点来看,规定,line1到line2逆时针为正,一
    般第一条线是要调整的线,第二条线是检测的线.
    :param line1: 实际上要检测的线
    :param line2: 铡刀能够检测的线,流程上是这条线先调整到跟铡刀平行,
    然后line1,就是实际要检测的中缝,再旋转到跟这条线重合.
    :return:
    """
    p1, p2 = line1
    p3, p4 = line2
    line1 = Line(p1[0], p1[1], p2[0], p2[1])
    line2 = Line(p3[0], p3[1], p4[0], p4[1])
    v1 = line1.vector()
    v2 = line2.vector()
    len1 = line1.length()
    len2 = line2.length()
    # 可以判断方向
    direction = v1[0] * v2[1] - v1[1] * v2[0]

    # 获取内积
    inner_mul = np.dot(v1, v2)
    # 获取模积
    model_mul = len1 * len2
    # 向量夹角公式
    cosx = inner_mul / model_mul
    # 防止计算误差,两条线平行的时候值超过1
    cosx = 1 if cosx > 1.0 else cosx
    top_middle_angle = np.arccos(cosx) * (180 / np.pi)
    top_middle_angle = top_middle_angle if direction > 0 else -top_middle_angle
    return round(top_middle_angle, 2)


def trans_k_to_rad(k):
    """
    将斜率转换为弧度
    :param k: 斜率k值
    :return:
    """
    return np.arctan(k)


def get_robot_pos(angleOffset, centerPoint):
    """
    获取机器人位置
    :param angleOffset: 角度偏移量
    :param centerPoint: 中心点
    :return:
    """
    robotAngle01 = angleOffset +rp.robot01Angle
    robotAngle02 = angleOffset + rp.robot02Angle
    imageCenter = get_two_points_middle(rp.imageP1,rp.imageP2)

    # 获取中心点和标定点的偏移量
    centerOffset02 =  calculate_rotated_offset( imageCenter,centerPoint,rp.rotateAngle02)
    centerOffset01 = calculate_rotated_offset(imageCenter,centerPoint,rp.rotateAngle01)

    offsetX01,offsetY01 = centerOffset01[0] / cp.mm2pix,centerOffset01[1] / cp.mm2pix
    offsetX02,offsetY02 = centerOffset02[0] / cp.mm2pix,centerOffset02[1] / cp.mm2pix


    # 获取机器人位置,目前就是默认偏移量x和y和机械手坐标同向,并且图像x和机械手x对应,图像y和机械手y对应
    # 机械手2的x方向是反的,所以这里的robotPos2的x坐标要用减
    # 机械手1的y方向是反的,x方向是对的,robotPos2的y坐标要用减
    robotPos1 = (rp.robot01P1[0] + offsetX01, rp.robot01P1[1] - offsetY01,
                 robotAngle01)
    robotPos2 = (rp.robot02P1[0] - offsetX02, rp.robot02P1[1] +offsetY02,
                 robotAngle02)

    return robotPos1, robotPos2


def calculate_rotated_offset(p1, p2, angle_deg, center=(0, 0)):
    """
    计算旋转后的坐标偏差变化
    :param p1: 点1坐标 (x1, y1)
    :param p2: 点2坐标 (x2, y2)
    :param angle_deg: 旋转角度（逆时针为正）
    :param center: 旋转中心，默认为原点
    :return: (新x偏差, 新y偏差)
    """
    # 原始偏差计算
    dx = p2[0] - p1[0]
    dy = p2[1] - p1[1]

    # 角度转弧度
    theta = math.radians(angle_deg)
    cos_theta = math.cos(theta)
    sin_theta = math.sin(theta)

    # 应用旋转矩阵[[cosθ, -sinθ], [sinθ, cosθ]]（网页1、71）
    new_dx = dx * cos_theta - dy * sin_theta
    new_dy = dx * sin_theta + dy * cos_theta

    return new_dx, new_dy

if __name__ == '__main__':
    leftTop = (375, 853)
    rightTop = (4814, 858)
    rightBottom = (4807, 4772)
    leftBottom = (357, 4768)
    widthMm = 1269
    heightMm = 1117.5

    mm2Pix = 3.502364
    heightWidthRatio = 0.98

    w1 = get_two_point_distance(leftTop, rightTop)
    w2 = get_two_point_distance(leftBottom, rightBottom)

    h1 = get_two_point_distance(leftTop, leftBottom)
    h2 = get_two_point_distance(rightTop, rightBottom)

    w = (w1 + w2) / 2
    h = (h1 + h2) / 2

    print(f"w1 = {w1:.2f},w2 = {w2:.2f},w = {w:.2f}")
    print(f"h1 = {h1:.2f},h2 = {h2:.2f},h = {h:.2f}")

    wPix = w / widthMm
    hPix = h / heightMm

    newHeightWidthRatio = hPix / wPix

    print(f"wPix = {wPix} ")
    print(f"hPix = {hPix} ")
    print(f"newHeightWidthRatio = {newHeightWidthRatio}")
    print(f"新旧高宽比对比: {newHeightWidthRatio / heightWidthRatio}")
